WOLF ROS2 publishers

WOLF ROS2 publishers are the procedures reponsible for generating/broadcasting information derived from the WOLF tree to the ROS2 ecosystem.

wolf::InterfaceBase

The abstract class wolf::InterfaceBase provides common functionality for both subscribers and publishers:

• Stores the node pointer, giving access to the logger and the timer with the corresponding getters.

• Stores the derived type (string).

• Stores the topic (string).

wolf::PublisherBase

All WOLF publishers must inherit from the base class wolf::PublisherBase defined in the wolf_ros2_node package.

class PublisherBase : public InterfaceBase
{
  protected:
    ProblemConstPtr   problem_;
    double            period_;
    rclcpp::Time      first_publish_time_;
    long unsigned int last_n_period_;

    std::thread pub_thread_;

    // PROFILING
    ProfilingUnit profiling_;

  public:
    PublisherBase(rclcpp::Node::SharedPtr _node, ProblemConstPtr _problem, const YAML::Node &_params)
        : InterfaceBase(_node, _params),
          problem_(_problem),
          period_(_params["period"].as<double>()),
          first_publish_time_(rclcpp::Time(0)),
          last_n_period_(0),
          profiling_(_params["period"].as<double>()){};

    virtual ~PublisherBase(){};

    virtual void run() final;
    virtual void stop() final;
    virtual void loop() final;

    virtual void publish() final;
    virtual void publishDerived() = 0;

    virtual bool ready();

    void printProfiling(std::ostream &stream = std::cout) const;
};

This abstract class provides common functionality for all WOLF publishers:

• WOLF problem pointer is stored in an attribute. The derived publisher can have access to it to publish the desired information.

• Own trhead as an attribute along with related methods run(), stop(), loop() and ready() which can be overritten in the derived class if needed.

• Profiling: A ProfilingUnit object is stored as an attribute . Profiling is automatically performed in publish() that calls the pure virtual method publishDerived(), which has to be implemented in the derived class.

Creating a publisher

To implement a new publisher, you need to derive from the wolf::PublisherBase class.

Definition

Analogously to other factory-based nodes in WOLF, the constructor should have a standard API and we provide the macro WOLF_PUBLISHER_CREATE to automatically implement the creator:

PublisherDerived(rclcpp::Node::SharedPtr _node,
                 ProblemConstPtr         _problem,
                 const YAML::Node&       _params);
WOLF_PUBLISHER_CREATE(PublisherDerived);

Also, add a ROS2 publisher attribute in the class definition. For example:

rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_state_block_;

Implementation

1. The constructor of the derived publisher should include:

• Getting desired parameters.

• Creation of the publisher providing the topic stored in InterfaceBase via getTopic().

1. Register the creator in the corresponding factory using the macro WOLF_REGISTER_PUBLISHER.

2. Implement the publishDerived() method that should get the desired information from the WOLF problem and publish it using the publisher attribute created in the constructor.

The following is an example of a derived publisher:

namespace wolf
{
PublisherStateBlock::PublisherStateBlock(rclcpp::Node::SharedPtr _node,
                                         ProblemConstPtr         _problem,
                                         const YAML::Node&       _params)
    : PublisherBase(_node, _problem, _params), msg_init_(false)
{
    sensor_ = _problem->findSensor(_params["sensor"].as<std::string>());
    assert(sensor_);
    key_ = _params["key"].as<char>();

    pub_state_block_ = _node->create_publisher<std_msgs::msg::Float64MultiArray>(getTopic(), 1);
}

void PublisherStateBlock::publishDerived()
{
    WOLF_WARN_COND(not sensor_->getStateBlock(key_), "StateBlock not found")
    if (not sensor_->getStateBlock(key_)) return;
    Eigen::VectorXd state_vec = sensor_->getStateBlock(key_)->getState();
    if (not msg_init_)
    {
        std_msgs::msg::MultiArrayDimension dim;
        dim.label  = sensor_->getName() + "/" + std::to_string(key_);
        dim.size   = state_vec.size();
        dim.stride = state_vec.size();
        state_msg_.layout.dim.push_back(dim);
        state_msg_.layout.data_offset = 0;
        state_msg_.data.resize(state_vec.size());
        msg_init_ = true;
    }
    Eigen::Map<Eigen::VectorXd> msg_map(state_msg_.data.data(), state_vec.size());
    msg_map = state_vec;

    pub_state_block_->publish(state_msg_);
}

WOLF_REGISTER_PUBLISHER(PublisherStateBlock)
}  // namespace wolf

Specification

You will have to create the corresponding schema file specifying the YAML parameters. The following is an example of a derived publisher schema:

follow: PublisherBase.schema

sensor:
  _type: std::string
  _mandatory: true
  _doc: "The sensor name from which the state block value should be published"

key:
  _type: char
  _mandatory: true
  _doc: "The key of the state block that should be published"

Finally, remember to add the new subscriber to the CMakeLists.txt of the correponding package.